/*
 * gyro_calibration.cpp
 *
 *  Created on: 26.08.2013
 *      Author: amalikov
 */

#include <nuttx/config.h>
#include <math.h>
#include <stdio.h>
#include <poll.h>
#include <fcntl.h>
#include <stdlib.h>
#include <visibility.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
//#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_gyro.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <mavlink/mavlink_log.h>
#include "gyro_calibration.h"

namespace {

struct gyro_offset {
	gyro_offset() : x_(0), y_(0), z_(0), t_(0), count_(0) {}

	void accumulate(double x, double y, double z, double t) {
		x_ += x;
		y_ += y;
		z_ += z;
		t_ += t;
		++count_;
	}

	double getx() const {
		return x_ / count_;
	}

	double gety() const {
		return y_ / count_;
	}

	double getz() const {
		return z_ / count_;
	}

	double gett() const {
		return t_ / count_;
	}

private:
	double x_;
	double y_;
	double z_;
	double t_;
	int count_;
};


void setGyroOffsets(float x, float y, float z) {
	int fd = open(GYRO_DEVICE_PATH, 0);

	struct gyro_scale gscale = {
		x,
		1.0f,
		y,
		1.0f,
		z,
		1.0f,
	};

	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
		warn("[fcs] WARNING: failed to set scale / offsets for gyro");

	close(fd);
}


void measureGyroOffsets(gyro_offset *result) {
	setGyroOffsets(0, 0, 0);

	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
	int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));

	const int calibration_count = 10000;

	for (int i = 0; i < calibration_count; ++i) {
		pollfd fds[1];
		fds[0].fd = sub_sensor_combined;
		fds[0].events = POLLIN;

		if (poll(fds, 1, 1000)) {
			static sensor_combined_s raw;
			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);

			static gyro_report gyro_report;
			orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);

			result->accumulate(raw.gyro_rad_s[0], raw.gyro_rad_s[1], raw.gyro_rad_s[2], gyro_report.temperature);
		}
	}

	orb_unsubscribe(sub_sensor_combined);
	orb_unsubscribe(sub_sensor_gyro);

	if (isfinite(result->getx()) && isfinite(result->gety()) && isfinite(result->getz())) {
		// set offsets to actual value
		setGyroOffsets((float)result->getx(), (float)result->gety(), (float)result->getz());
	} else {
		warn("[fcs] gyro calibration FAILED (NaN)");
	}
}

}


void do_gyro_calibration(int mavlink_fd) {
	sleep(30);

	mavlink_log_info(mavlink_fd, "[fcs] starting gyro calibration");
	printf("[fcs] starting gyro calibration\n");

	gyro_offset result;
	measureGyroOffsets(&result);

	static char result_str[256] = {0};
	sprintf(result_str, "t=%.1f x=%.5f y=%.5f z=%.5f", result.gett(), result.getx(), result.gety(), result.getz());

	mavlink_log_info(mavlink_fd, "[fcs] gyro calibration finished: %s", result_str);
	printf("[fcs] gyro calibration finished: %s\n", result_str);

	FILE *f = fopen("/fs/microsd/senscal.log", "a+");
	if (f != NULL) {
		fputs(result_str, f);
		fputs("\r\n", f);
		fclose(f);
	} else
		mavlink_log_info(mavlink_fd, "[fcs] writing to log failed");

}
